# -*- coding: utf-8 -*-
import time
import rospy
from typing import Mapping
from unicodedata import name
from geometry_msgs.msg import PoseStamped, Point
import roslibpy
import math
import sys

other_iris_pose = PoseStamped()

class sub_other_uav_info():
    def __init__(self, ros_host, iris_num):
        # 1. 启动redis和roslibpy连接
        self.ros_client = None
        self.ros_s = False
        try:
            self.ros_client = roslibpy.Ros(host=ros_host, port=9090)
            self.ros_client.run()
            print('ROS OK!')
            self.ros_s = True
        except:
            print("ROS connect error!")
            

        self.rostopic_n = 0
        # 2.订阅的其他无人机的vision话题, 利用roslibpy
        sub_other_topic = '/iris_' + str(iris_num) + '/mavros/vision_pose/pose'
        self.rostopic_list = [ sub_other_topic, '/mavros/imu/data',   '/mavros/local_position/pose',  '/mavros/battery']
        self.rostopic_type = ['geometry_msgs/PoseStamped', 'sensor_msgs/Imu', 'geometry_msgs/PoseStamped',    'sensor_msgs/BatteryState']
        
        self.ros_listener = roslibpy.Topic(self.ros_client, self.rostopic_list[self.rostopic_n], self.rostopic_type[self.rostopic_n], queue_size=100)
        self.ros_listener.subscribe(self.ros_call)
        
        # 3.rospy发布信息给当前主机
        rospy.init_node('roslibpy_communicate')
        pub_topic_name = '/transfer/iris_' + str(iris_num) + '/mavros/vision_pose/pose'
        self.position_pub = rospy.Publisher(pub_topic_name, PoseStamped, queue_size=1)
        # self.position_pub = roslibpy.Topic(self.ros_client, pub_topic_name, 'geometry_msgs/PoseStamped')
        # self.position_pub.advertise()
        
        
        
        

        
    def ros_call(self, msg):
        if self.rostopic_n==0: 
            print(msg)
            
            other_iris_pose.header.frame_id = 'world'
            other_iris_pose.header.stamp = rospy.Time.now()
            
            other_iris_pose.pose.position.x = msg['pose']['position']['x']
            other_iris_pose.pose.position.y = msg['pose']['position']['y']
            other_iris_pose.pose.position.z = msg['pose']['position']['z']

            other_iris_pose.pose.orientation.w = msg['pose']['orientation']['w']
            other_iris_pose.pose.orientation.x = msg['pose']['orientation']['x']
            other_iris_pose.pose.orientation.y = msg['pose']['orientation']['y']
            other_iris_pose.pose.orientation.z = msg['pose']['orientation']['z']
                    
            try:
                self.position_pub.publish(other_iris_pose)
            except Exception as e:
                print("Failed to publish msg:", str(e))
            
        elif self.rostopic_n==1:
            print(msg)
            seq_id = msg['header']['seq']
            # self.ros_topic_dat = (dat['pose']['pose']['position'])
            print(seq_id)
        elif self.rostopic_n==2:
            print(msg)
            seq_id = msg['header']['seq']
            # self.ros_topic_dat = (dat['pose']['pose']['position'])
            print(seq_id)
        elif self.rostopic_n==3:
            self.ros_topic_dat = (str('电池电压：') + str(msg['voltage']))

if __name__ == "__main__":
    other_iris_id = 1
    other_iris_ip = '192.168.31.85' # CPS_WIFI_2
    # iris_ip = '192.168.10.230' # HM_5G
    t = sub_other_uav_info(other_iris_ip, other_iris_id)
    
    # rate = rospy.Rate(30) 
    while True:
        pass
        # try:
        #     t.position_pub.publish(other_iris_pose)
        # except Exception as e:
        #     pass
        #     # print("Failed to publish msg:", str(e))
        
        # rate.sleep()



